package es.uji.viselab.kinematics;

import javax.media.j3d.*;
import javax.vecmath.*;

import es.uji.viselab.math.Pose;

/* Consider this class like Matrix4d, but with more methods useful for transformations
 * For transformations I use the java3D Transform3D 
 */

public class TMatrix4f {

	final static double PI = Math.PI;
	private Transform3D value;

	// Default constructor, make a identity matrix 4x4
	public TMatrix4f() {
		value = new Transform3D();
	}

	public TMatrix4f(Matrix4f matrix) {
		value = new Transform3D(matrix);
	}

	// I set a Matrix to my Transform3D
	public void set(Matrix4f m4d) {
		value.set(m4d);
	}

	// I add another Matrix to my Transform3D
	public void add(Matrix4d M1) {
		Matrix4d M2 = new Matrix4d();
		value.get(M2);
		M1.mul(M2);
		value.set(M1);
	}

	// Like before, I accept a Transform3D matrix
	public void add(Transform3D T) {
		Matrix4d M = new Matrix4d();
		T.get(M);
		add(M);
	}

	/*
	 * When I have a vector 4x1 of points in the space and I want aplicate the
	 * transformation matrix this is the function I have to use This returns the
	 * space points relative for the transform matrix
	 */
	public Vector3d mult(Vector4d V) {
		Matrix4d M = new Matrix4d();
		Vector4d V4out = new Vector4d();
		Vector3d V3out = new Vector3d();
		value.get(M);
		V4out.x = M.getElement(0, 0) * V.x + M.getElement(0, 1) * V.y
				+ M.getElement(0, 2) * V.z + M.getElement(0, 3) * V.w;
		V4out.y = M.getElement(1, 0) * V.x + M.getElement(1, 1) * V.y
				+ M.getElement(1, 2) * V.z + M.getElement(1, 3) * V.w;
		V4out.z = M.getElement(2, 0) * V.x + M.getElement(2, 1) * V.y
				+ M.getElement(2, 2) * V.z + M.getElement(2, 3) * V.w;
		V4out.w = M.getElement(3, 0) * V.x + M.getElement(3, 1) * V.y
				+ M.getElement(3, 2) * V.z + M.getElement(3, 3) * V.w;
		V3out.x = V4out.x / V4out.w;
		V3out.y = V4out.y / V4out.w;
		V3out.z = V4out.z / V4out.w;

		return V3out;
	}

	public Transform3D getT() {
		return value;
	}

	public Matrix4d getM() {
		Matrix4d M = new Matrix4d();
		value.get(M);
		return M;
	}

	public void rotX(double angle) {
		Transform3D temp = new Transform3D();
		temp.rotX(angle);
		add(temp);
	}

	public void rotY(double angle) {
		Transform3D temp = new Transform3D();
		temp.rotY(angle);
		add(temp);
	}

	public void rotZ(double angle) {
		Transform3D temp = new Transform3D();
		temp.rotZ(angle);
		add(temp);
	}

	public void rotGradX(double angle) {
		rotX(angle);
	}

	public void rotGradY(double angle) {
		rotY(angle );
	}

	public void rotGradZ(double angle) {
		rotZ(angle);
	}

	// Apply in the actual tranformation matrix a translation
	public void trans(double x, double y, double z) {
		Transform3D temp = new Transform3D();
		Vector3d vector = new Vector3d(x, y, z);
		temp.setTranslation(vector);
		add(temp);
	}

	public void erase() {
		value = new Transform3D();
	}

	public static TMatrix4f getIdentity() {
		TMatrix4f transforMatrix = new TMatrix4f();
		Matrix4f m4d = new Matrix4f();
		m4d.m00=1;
		m4d.m10=0;
		m4d.m20=0;
		m4d.m30=0;
		m4d.m01=0;
		m4d.m11=1;
		m4d.m21=0;
		m4d.m31=0;
		m4d.m02=0;
		m4d.m12=0;
		m4d.m22=1;
		m4d.m32=0;
		m4d.m03=0;
		m4d.m13=0;
		m4d.m23=0;
		m4d.m33=1;
		transforMatrix.set(m4d);
		return transforMatrix;
	}

	public Vector3d mult(Vector3d v) {
		Vector4d vector = new Vector4d(v.x,v.y,v.z,1);
		return mult(vector);
	}

}